#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonar,          sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  leftSwitch,     sensorTouch)
#pragma config(Sensor, dgtl8,  rightSwitch,    sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int speed=127;
void forward(int revs){
  int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]<turns){
if(SensorValue[rightEncoder]>(SensorValue[leftEncoder])){
  motor[rightMotor]=110;
  motor[leftMotor]=127;
}
 else if((SensorValue[leftEncoder])>SensorValue[rightEncoder]){
   motor[rightMotor]=127;
   motor[leftMotor]=105;
  }

else{
  motor[rightMotor]=80;
  motor[leftMotor]=80;
}
}
}

void backwards(int revs){
  int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]>-turns){
if(SensorValue[rightEncoder]>(SensorValue[leftEncoder])){
  motor[rightMotor]=-110;
  motor[leftMotor]=-127;
}
 else if((SensorValue[leftEncoder])>SensorValue[rightEncoder]){
   motor[rightMotor]=-127;
   motor[leftMotor]=-105;
  }

else{
  motor[rightMotor]=-80;
  motor[leftMotor]=-80;
}
}
}

void turnRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[leftEncoder]<195){
  motor[leftMotor]=speed;
  motor[rightMotor]=-speed;
 }
}
void turnLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]<720){
  motor[leftMotor]=-speed;
  motor[rightMotor]=speed;
  wait1Msec(5);
 }
}

void slightRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[leftEncoder]>0){
  backwards(2);
  wait1Msec(3);
}
while(SensorValue[rightEncoder]>0){
  motor[rightMotor]=-speed;
  motor[leftMotor]=speed;
  wait1Msec(3);
}
}

void slightLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[rightEncoder]>0){
  backwards(2);
  wait1Msec(3);
}
while(SensorValue[rightEncoder]>0){
  motor[rightMotor]=speed;
  motor[leftMotor]=-speed;
  wait1Msec(3);
}
}
task main(){
 while(1==1){
while(SensorValue[sonar]>100|| SensorValue[sonar]<0){
 if(SensorValue[leftSwitch]==0 && SensorValue[rightSwitch]==0){
 forward(1);
}
 else if(SensorValue[leftSwitch]==1){
   slightRight();
 }
 else if(SensorValue[rightSwitch]==1){
 slightLeft();
}
}
}
}
